%% 程序初始化，清空工作空间，缓存，
clear all;
close all;
clc;
% 读取Octave控制数据库
pkg load control;
% 读取Octave优化求解器数据库
pkg load optim;
%% 定义系统的状态方程
%构建系统矩阵A，5*5 5行5列
A = [0 0 0 0 0;0 0 0 0 0;0 0 0 0 0;1 -1 0 0 0;0 1 -1 0 0];
% 计算A矩阵维度n，确定A的行数
n = size (A,1);
%构建系统的输入矩阵B，5*3 5行3列B
B = [1 0 0;0 1 0;0 0 1;0 0 0;0 0 0 ];
%计算系统输入矩阵B的维度p，确定B的列数
p = size(B,2);

%% 对系统进行离散，确定离散出来的状态方程
%离散时间的确定
Ts = 0.02;
%连续系统转化成离散系统
sys_d = c2d(ss(A,B),Ts);
%提取离散系统A矩阵
A = sys_d.a;
%提取离散系统B矩阵
B = sys_d.b;

%% 对权重系统进行设计
%设计状态权重矩阵Q 5*5 5行5列
Q = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 1 0;0 0 0 0 1];
%设计终止状态权重矩阵S 5*5 5行5列
S = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 1 0;0 0 0 0 1];
%设计输入系数权重矩阵R 3*3矩阵 3行3列
R = [1 0 0;0 1 0;0 0 1];

%% 对参考状态量进行设计，车辆完成协同变道以后的期望状态量进行确定
%系统状态量参考值
xd = [20;20;20;22;22];
% 构建目标转移矩阵
AD = eye(n);
% 计算目标输入
ud = mldivide(B,(eye(n)-A)*xd);
ud1 = ud(1);
ud2 = ud(2);
ud3 = ud(3);

%% 对整个系统进行初始化
% 初始化车辆的运动状态
x0 = [16;18;17;18;19];x = x0;
% 初始化增广状态矩阵
xa = [x; xd];
% 初始化系统输入
u0 = [0;0;0]; u = u0;

%% 对系统的约束进行定义
%输入下限
u_low = [-5;-5;-5];
% 输入上限
u_high = [3;3;3];
% 状态下限
x_low = [-inf;-inf;-inf;-inf;-inf];
% 状态上限
x_high = [inf;inf;inf;inf;inf];
% 增广状态下限
xa_low = [x_low;-inf;-inf;-inf;-inf;-inf];
% 增广状态上限
xa_high = [x_high;inf;inf;inf;inf;inf];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 100;
% 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step
x_history = zeros(n,k_steps);
% 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step
u_history = zeros(p,k_steps);
% 定义x_history_noconstraint零矩阵，用于储存系统状态结果，维度n x k_step
x_history_noconstraint = zeros(n,k_steps);
% 定义u_history_noconstraint零矩阵，用于储存系统输入结果，维度p x k_step
u_history_noconstraint = zeros(p,k_steps);
% 定义预测区间
N_P = 20;

% 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud
[Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd);

% 调用模块[F4]计算二次规划需用到的矩阵
[Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P);

% 调用模块[F6]计算含约束二次规划需用到的矩阵
[M,Beta_bar,b]= F6_MPC_Matrices_Constraints(xa_low,xa_high,u_low,u_high,N_P,Phi,Gamma);

% for循环开始仿真 有约束
for k = 1 : k_steps
% 调用模块[F7]计算系统系统控制（输入增量），这里需要注意的是在F7中，设置只输出delta_U
[delta_U, delta_u]= F7_MPC_Controller_withConstriants(xa,F,H,M,Beta_bar,b,p);
%计算输出，只选取求解的delta_U的第一组控制量。注意，这里是控制量的变化量
delta_u1 = delta_U(1);
delta_u2 = delta_U(2);
delta_u3 = delta_U(3);
%更新这一时刻的控制量
u(1) = delta_u1+ud1;
u(2) = delta_u2+ud2;
u(3) = delta_u3+ud3;
% 系统输入代入系统方程，计算系统响应
u = [u(1);u(2);u(3)];
x = A * x + B * u;
% 更新增广矩阵xa
xa = [x; xd];
% 保存系统状态到预先定义矩阵的相应位置
x_history (:,k+1) =  x;
% 保存系统输入到预先定义矩阵的相应位置
u_history (:,k) = u ;
end
